// Utilities for sensor measurements

// Removes noise from sensor data
int smooth(int rawIn, int *smooth_array) {
  long total = 0;
  static int i;

  i = (i + 1) % 16;
  smooth_array[i] = rawIn;
  for (int j = 0; j < 16; j++) total += smooth_array[j];
  return total >> 4 ;
}

// Used to average the sensors measurements when everything is level
int findZero(int channel) {
  long zero = 0;
  
  for (int i=0; i< 32; i++) zero += analogRead(channel);
  return zero >> 5;
}

// Allows user to zero gyros on command
void zeroGyros() {
  do {
    rollGyroZero = findZero(ROLLRATEPIN);
  } while (abs(analogRead(ROLLRATEPIN) - rollGyroZero) > ZEROLIMIT);
  
  do {
    pitchGyroZero = findZero(PITCHRATEPIN);
  } while (abs(analogRead(PITCHRATEPIN) - pitchGyroZero) > ZEROLIMIT);

  do {
    yawGyroZero = findZero(YAWRATEPIN);
  } while (abs(analogRead(YAWRATEPIN) - yawGyroZero) > ZEROLIMIT);
}

// Allows user to zero accelerometers on command
void zeroAccelerometers() {
  do {
    xAccelZero = findZero(XACCELPIN);
  } while (abs(analogRead(XACCELPIN) - xAccelZero) > ZEROLIMIT);
  
  do {
    yAccelZero = findZero(YACCELPIN);
  } while (abs(analogRead(YACCELPIN) - yAccelZero) > ZEROLIMIT);
  
  do {
    zAccelZero = findZero(ZACCELPIN);
  } while (abs(analogRead(ZACCELPIN) - zAccelZero) > ZEROLIMIT);
}
